//创建action客户端，发送导航目标点，完成多点导航
// 2025.7.29
//jerry
#include <vector>
#include "nav2_msgs/action/follow_waypoints.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"

using FollowWaypoints = nav2_msgs::action::FollowWaypoints;
using GoalHandle = rclcpp_action::ClientGoalHandle<FollowWaypoints>;

class WaypointsNavigator : public rclcpp::Node {
public:
    WaypointsNavigator() : Node("waypoints_navigator") {
        client_ = rclcpp_action::create_client<FollowWaypoints>(this, "/FollowWaypoints");
        
        // 初始化示例航点 (x, y, 朝向)
        addWaypoint(2.0, 1.0, 1.0);  // w=1.0 表示0弧度
        addWaypoint(3.0, -2.0, 0.707); // 45度
        addWaypoint(0.0, 0.0, -0.5);  // 120度
        
        // 启动导航
        // timer_ = create_wall_timer(
        //     std::chrono::seconds(2), 
        //     [this]() { sendWaypoints(); });
    }

     void sendWaypoints() {
        // timer_->cancel(); // 只发送一次

        while (rclcpp::ok() && !client_->wait_for_action_server(std::chrono::seconds(2))) {
            RCLCPP_WARN(get_logger(), "等待Action 服务上线。");
            continue;
        }
        if (!rclcpp::ok()) {
            return;  // 如果ROS已关闭，直接退出
        }
        // while (!client_->wait_for_action_server(std::chrono::seconds(5))) {
        //     RCLCPP_INFO(get_logger(), "等待Action 服务上线。");
        // }

        auto goal = FollowWaypoints::Goal();
        goal.poses = waypoints_;

        auto send_options = rclcpp_action::Client<FollowWaypoints>::SendGoalOptions();
        
        send_options.goal_response_callback = 
            [this](std::shared_future<GoalHandle::SharedPtr> future) {
                auto handle = future.get();
                if (!handle) {
                    RCLCPP_ERROR(get_logger(), "目标被拒绝");
                }
            };

        send_options.feedback_callback = 
            [this](GoalHandle::SharedPtr, 
                   const std::shared_ptr<const FollowWaypoints::Feedback> feedback) {
                RCLCPP_INFO(get_logger(), "当前航点: %u/%u", 
                           feedback->current_waypoint + 1, 
                           waypoints_.size());
            };

        send_options.result_callback = 
            [this](const GoalHandle::WrappedResult& result) {
                switch(result.code) {
                    case rclcpp_action::ResultCode::SUCCEEDED:
                        RCLCPP_INFO(get_logger(), "所有航点完成!");
                        break;
                    case rclcpp_action::ResultCode::ABORTED:
                        RCLCPP_ERROR(get_logger(), "导航中止于航点 %d", 
                                    result.result->missed_waypoints.front());
                        break;
                    case rclcpp_action::ResultCode::CANCELED:
                        RCLCPP_WARN(get_logger(), "导航被取消");
                        break;
                    default:
                        RCLCPP_ERROR(get_logger(), "未知错误");
                }
                rclcpp::shutdown();
            };

        RCLCPP_INFO(get_logger(), "开始执行 %d 个航点", waypoints_.size());
        client_->async_send_goal(goal, send_options);
    }
    
private:

    rclcpp_action::Client<FollowWaypoints>::SharedPtr client_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::vector<geometry_msgs::msg::PoseStamped> waypoints_;

    void addWaypoint(float x, float y, float w) {
        geometry_msgs::msg::PoseStamped pose;
        pose.header.frame_id = "map";
        pose.pose.position.x = x;
        pose.pose.position.y = y;
        pose.pose.orientation.w = w;
        waypoints_.push_back(pose);
    }
};
int main(int argc, char** argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<WaypointsNavigator>();
    std::signal(SIGINT, [](int) { rclcpp::shutdown(); });
    node->sendWaypoints();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}